#include "Pid.h"
using namespace std;
using namespace PID;

PidCore::PidCore( d_PidDateType PID_add,  f_PidDateType PID_p,  f_PidDateType PID_i,  f_PidDateType PID_d)
{
    m_currentError = 0.0;
    m_lastError = 0.0;
    m_prevError = 0.0;
    m_p = 0.0;
    m_i = 0.0;
    m_d = 0.0;
    m_pidKp = PID_p;
    m_pidKi = PID_i;
    m_pidKd = PID_d; 
    m_pidAddMax = 0.1;
    m_pidAddMin = -0.1;
    m_pidLinevelocity = 0.1;
    m_pidAngularvelocity = PID_add;
}
b_PidDateType PidCore::Beam(f_PidDateType x, f_PidDateType y, f_PidDateType z)
{
    if(((1-x) > 0) && ((2 *x + 2 * y) > 0) && ((2 * x -2 * y) > 0))
    {
        cout<<"ture"<<endl;
        return true;
        
    }else{
        cout<<"false"<<endl;
        return false;
        
    }

}

b_PidDateType PidCore::Motor_Ctl(PidCore &PID_M, f_PidDateType leftX, f_PidDateType leftY, f_PidDateType leftZ,
                                       f_PidDateType rightX, f_PidDateType rightY, f_PidDateType rightZ)
{
   b_PidDateType BeamFlagLeft;
   b_PidDateType BeamFlagRight;
   BeamFlagLeft = PID_M.Beam(leftX, leftY, leftZ);
   BeamFlagRight = PID_M.Beam(rightX, rightY, rightZ);
   if(BeamFlagLeft && BeamFlagRight)
   {
       f_PidDateType error = 0;
       error = leftX - rightX;	
       PID_M.m_prevError = PID_M.m_lastError; 
       PID_M.m_lastError = PID_M.m_currentError; 
       PID_M.m_currentError = error; 

       PID_M.m_p = PID_M.m_pidKp * (PID_M.m_currentError - PID_M.m_lastError); 
       PID_M.m_i = PID_M.m_pidKi * PID_M.m_currentError; 
       PID_M.m_d = PID_M.m_pidKd * (PID_M.m_currentError - 2 * PID_M.m_lastError + PID_M.m_prevError); 
    

       PID_M.m_pidAngularvelocity = PID_M.m_p + PID_M.m_i + PID_M.m_d + PID_M.m_pidAngularvelocity; 

       if(PID_M.m_pidAngularvelocity >= PID_M.m_pidAddMax)
       {
           PID_M.m_pidAngularvelocity = PID_M.m_pidAddMax;
       }
       if(PID_M.m_pidAngularvelocity <= PID_M.m_pidAddMin)
       {
        PID_M.m_pidAngularvelocity = PID_M.m_pidAddMin;
       }
       PID_M.m_pidLinevelocity = 0.05;
       cout<<"PID_M.pidAngularvelocity =  "<<PID_M.m_pidAngularvelocity<<endl;
       cout<<"true"<<endl;
       return true;	
   }
    if(BeamFlagLeft)
    {
        PID_M.m_pidLinevelocity = 0.05;
        PID_M.m_pidAngularvelocity = 0.15;
        cout<<"true"<<endl;
        return true;
    }
    else if(BeamFlagRight)
    {
        PID_M.m_pidLinevelocity = 0.05;
        PID_M.m_pidAngularvelocity = -0.15;
        cout<<"true"<<endl;
        return true;
    }
    else{
        return false;
   }

}
